#include "main.h"
#include "tinymt32.clh"

void perturb (tinymt32wp_t tiny,__global tinymt32wp_t *d_status, float *data, float *eva, float *eve,uint n, uint d, float *return_data);
void theorize(tinymt32wp_t tiny,__global tinymt32wp_t *d_status, float *mu, float *eva, float *eve, uint n, uint d, float *return_data);
tinymt32wp_t init_u01(__global tinymt32wp_t *d_status);
float u01_long(tinymt32wp_t tiny, __global tinymt32wp_t *d_status);
#define u01() u01_long(tiny,d_status)   
void mean(float *data, uint n, uint d, float *r);
void covariance(float *data, uint n, uint d, float *r);
void svd(float *data, uint d, float *return_values, float *return_vectors);

tinymt32wp_t init_u01(__global tinymt32wp_t *d_status) {
	tinymt32wp_t tiny;
	const size_t id = tinymt_get_sequential_id();
	tinymt32_status_read(&tiny, d_status);
	tinymt32_init(&tiny, SEED + id); //USING get_group_id(0) as our SEED...
	tinymt32_status_write(d_status,&tiny);
	return tiny;
}

float u01_long(tinymt32wp_t tiny, __global tinymt32wp_t *d_status) {
	tinymt32_status_read(&tiny, d_status);
	float r=tinymt32_single01(&tiny);
	tinymt32_status_write(d_status,&tiny);
	return r;
}

float n01(float u1, float u2, bool use_sin);
float n01(float u1, float u2, bool use_sin) {

	if (use_sin)
			return sqrtf(-2*logf(u1))*sin(2*PI*u2);
	else
			return sqrtf(-2*logf(u1))*cos(2*PI*u2);

}
void merge(float numbers[], float temp[], int left, int mid, int right);//, uint index[], uint index_tmp[]);
void merge(float numbers[], float temp[], int left, int mid, int right) //, uint index[], uint index_tmp[])
{
  int i, left_end, num_elements, tmp_pos;
 
  left_end = mid - 1;
  tmp_pos = left;
  num_elements = right - left + 1;
 
  while ((left <= left_end) && (mid <= right))
  {
    if (numbers[left] <= numbers[mid])
    {
      temp[tmp_pos] = numbers[left];
      //index_tmp[tmp_pos]=index[left];
      tmp_pos = tmp_pos + 1;
      left = left +1;
    }
    else
    {
      temp[tmp_pos] = numbers[mid];
      //index_tmp[tmp_pos]=index[mid];
      tmp_pos = tmp_pos + 1;
      mid = mid + 1;
    }
  }
 
  while (left <= left_end)
  {
    temp[tmp_pos] = numbers[left];
    //index_tmp[tmp_pos]=index[left];
    left = left + 1;
    tmp_pos = tmp_pos + 1;
  }
  while (mid <= right)
  {
    temp[tmp_pos] = numbers[mid];
    //index_tmp[tmp_pos]=index[mid];
    mid = mid + 1;
    tmp_pos = tmp_pos + 1;
  }
 
  for (i=0; i <= num_elements; i++)
  {
    numbers[right] = temp[right];
    //index[right]=index_tmp[right];
    right = right - 1;
  }
}
void m_sort(float numbers[], float temp[], int left, int right);//, uint index[], uint index_tmp[]);

void m_sort(float numbers[], float temp[], int left, int right)//, uint index[], uint index_tmp[])
{
  int mid;
 
  if (right > left)
  {
    mid = (right + left) / 2;
    m_sort(numbers, temp, left, mid); //, index, index_tmp);
    m_sort(numbers, temp, mid+1, right); //, index, index_tmp);
 
    merge(numbers, temp, left, mid+1, right);//, index, index_tmp);
  }
}
void mergeSort(float *d);//,uint n);

void mergeSort(float *d) {
	float temp[TRIALS];

	//uint *index=new uint[n], index_tmp[n]; 
//	for (uint i=0;i<n;i++)
//			index[i]=i;

	m_sort(d, temp, 0, TRIALS - 1);//,index,index_tmp);

	//return index;	
}
void svd(float *data, uint d, float *return_values, float *return_vectors) {
//uint n=d->n;
//struct svd *ret=new svd;

if (1) {
//	ret->vectors=init_Rmn(1,1,0);
//	ret->values=init_Rn(1,0);
	//ret->vectors->f[0]->f[0]=1;
	return_vectors[0]=1;
//	ret->values->f[0]=d->f[0]->f[0];
	return_values[0]=data[0];
	//return ret;
	return;
}



//struct Rmn *A=init_Rmn(2*n,n,0);

float A[2*MAX_DIM*MAX_DIM];

//struct Rn *S2=init_Rn(n,0);

float S2[MAX_DIM];

for (uint i=0;i<d;i++)
	for (uint j=0;j<d;j++)
		//A->f[i]->f[j]=d->f[i]->f[j];
		A[i*d+j]=data[i*d+j];


  uint  i, j, k, EstColRank = d, RotCount = d, SweepCount = 0,
       slimit = (d<120) ? 30 : d/4;
  double eps = 1e-15, e2 = 10.0*d*eps*eps, tol = 0.1*eps, vt, p, x0,
       y0, q, r, c0, s0, d1, d2;

  for (i=0; i<d; i++) { 
	for (j=0; j<d; j++) 
//	 A->f[d+i]->f[j] = 0.0;
		A[(d+i)*d+j]=0.0; 
	//A->f[d+i]->f[i] = 1.0; 
	A[(d+i)*d+j]=1;
  }
  while (RotCount != 0 && SweepCount++ <= slimit) {
    RotCount = EstColRank*(EstColRank-1)/2;
    for (j=0; j<EstColRank-1; j++) 
      for (k=j+1; k<EstColRank; k++) {
	p = q = r = 0.0;
	for (i=0; i<d; i++) {
	  //x0 = A->f[i]->f[j]; 
	  x0=A[i*d+j];
	  //y0 = A->f[i]->f[k];
	  y0=A[i*d+k];
	  p += x0*y0; q += x0*x0; r += y0*y0;
	}
       // S2->f[j] = q; S2->f[k] = r;
       S2[j]=q; S2[k]=r;
	if (q >= r) {
	  //if (q<=e2*S2->f[0] || fabs(p)<=tol*q)
	  if (q<=e2*S2[0] || fabs(p)<=tol*q)
	    RotCount--;
	  else {
	    p /= q; r = 1.0-r/q; vt = sqrt(4.0*p*p+r*r);
	    c0 = sqrt(0.5*(1.0+r/vt)); s0 = p/(vt*c0);
	    for (i=0; i<2*d; i++) {
	      //d1 = A->f[i]->f[j]; d2 = A->f[i]->f[k];
	      d1=A[i*d+j]; d2=A[i*d+k];
	      //A->f[i]->f[j] = d1*c0+d2*s0; A->f[i]->f[k] = -d1*s0+d2*c0;
	      A[i*d+j]=d1*c0+d2*s0; A[i*d+k]=-d1*s0+d2*c0;
	    }
	  }
	} else {
	  p /= r; q = q/r-1.0; vt = sqrt(4.0*p*p+q*q);
	  s0 = sqrt(0.5*(1.0-q/vt));
	  if (p<0.0) s0 = -s0;
	  c0 = p/(vt*s0);
	  for (i=0; i<2*d; i++) {
	    //d1 = A->f[i]->f[j]; d2 = A->f[i]->f[k];
	    d1=A[i*d+j]; d2=A[i*d+k];
	    //A->f[i]->f[j] = d1*c0+d2*s0; A->f[i]->f[k] = -d1*s0+d2*c0;
	    A[i*d+j]=d1*c0+d2*s0; A[i*d+k]=-d1*s0+d2*c0;
	  }
	}
      }
    while (EstColRank>2 && S2[EstColRank-1]<=S2[0]*tol+tol*tol) EstColRank--;
  }
//  if (SweepCount > slimit)
  //  printf("Warning: Reached maximum number of sweeps (%d) in SVD routine...\n"
//	   ,slimit);

//struct Rmn *eigenvectors=init_Rmn(n,n,0);

for (i=0;i<d;i++)  {
	S2[i]=sqrt(S2[i]);
	return_values[i]=S2[i]; //(from below)
	for (j=0;j<d;j++)
		//eigenvectors->f[i]->f[j]=A->f[n+i]->f[j];
		return_vectors[i*d+j]=A[(d+i)*d+j];
}

//destroy_Rmn(A);



//ret->vectors=eigenvectors;
//ret->values=S2;

//return ret;

}

void mean(float *data,uint n, uint d, float *r) { //get the mean of n samples in the d'th dimension
	for (uint i=0;i<d;i++) {
		r[i]=0;
		for (uint j=0;j<n;j++)
			r[i]+=data[d*j+i];
		r[i]/=(float)n;
	}
}

void covariance(float *data, uint n, uint d,  float *r) { // get the variance of the n samples in d dimensions
	
	float x,y,xy;

	for (uint i=0;i<d;i++) {
			for (uint j=0;j<=i;j++) {
				x=0,y=0,xy=0;
				for (uint k=0;k<n;k++) {
					x+=data[k*d+i];
					y+=data[k*d+j];
					xy+=data[k*d+i]*data[k*d+j];
					
				}
				x/=n;
				y/=n;
				xy/=n;
				r[i*d+j]=xy-x*y;
				r[j*d+i]=xy-x*y;
			}

	}
}


void theorize(tinymt32wp_t tiny,__global tinymt32wp_t *d_status,float *mu, float *eva, float *eve, uint n, uint d, float *return_data) {


	//struct Rn *eigenvalues=ed->values;
	//struct Rmn *eigenvectors=ed->vectors;//FIXME

//	uint n=mu->n;
	


//	struct Rmn *d=init_Rmn(m,n,0);

//	float *iids=new float[n];
	float iids[MAX_SAMPLES];
	float u1=0,u2=0;

for (uint i=0;i<n;i++) {

	for (uint j=0;j<d;j++) {
		if (j%2==0)
			u1=u01(), u2=u01();
		iids[j]=n01(u1,u2,j%2);
	}

	for (uint j=0;j<d;j++) {
		//d->f[i]->f[j]=mu->f[j];
		return_data[i*d+j]=mu[j];
		for (uint k=0;k<d;k++)
			//d->f[i]->f[j]+=sqrt(eigenvalues->f[k])*eigenvectors->f[j]->f[k]*iids[k];
			return_data[i*d+j]+=sqrt(eva[k])*eve[j*d+k]*iids[k];
	}


}

//	delete iids;
	//return d;
}

void perturb_mu (tinymt32wp_t tiny, __global tinymt32wp_t *d_status, float *mu, float *eva, float *eve, uint n, uint d);
void perturb_mu (tinymt32wp_t tiny, __global tinymt32wp_t *d_status, float *mu, float *eva, float *eve, uint n, uint d) { // THIS CAN BE USER-SPECIFIED
	//owing to the need to preserve the covariance matrix in our perturbed parameters, what i'm actually doing here is taking the (mu,s^2) parameters, generating a dataset of n samples, then setting mu to be the mean of that dataset, which should be slightly different from mu. if we need more variance, just modify n accordingly.
	float theorized_data[MAX_DIM*MAX_SAMPLES]; 
	theorize(tiny,d_status,mu, eva, eve, n, d, theorized_data);
	mean(theorized_data,n,d,mu);
}

float distance_f(float *emperical, float *theorized, uint n, uint d);
float distance_f(float *emperical, float *theorized, uint n, uint d) {

	float sum=0;
	for (uint i=0;i<n*d;i++)
			sum+=powf(emperical[i]-theorized[i],2);

	return sum;
}




float theorize_determinant(float *eva, uint d);
float theorize_determinant(float *eva, uint d) {
	float f=1;
	for (uint i=0;i<d;i++)
		f*=eva[i];
	return f;
}

void multiply(float *a, uint n_1, uint m_1, float *b, uint n_2, uint m_2, float *c);
void multiply(float *a, uint n_1, uint m_1, float *b, uint n_2, uint m_2, float *c) { //c is return data, must be n_1xm_2 dimension
	//assert(m_1==n_2);

	for (uint i=0;i<n_1;i++)
		for (uint j=0;j<m_2;j++) {
			 c[i*n_1+j]=0; // may not need this if init_Rmn is initializing properly..
			for (uint k=0;k<m_1;k++) 
				c[i*n_1+j]+=a[i*m_1+k]*b[k*m_2+j];
		}
}
void theorize_inverse_covariance(float *eva, float *eve, float *return_data, uint d);
void theorize_inverse_covariance(float *eva, float *eve, float *return_data, uint d) {
	//struct Rmn *vectors=ed->vectors;
	//struct Rn *values=ed->values;

	//struct Rmn *data=init_Rmn(vectors->m,vectors->n,0);

	float data[d*d];

	//struct Rmn *r=NULL;
// idea is to multiply: vectors*values*vectors^T... let's start with values*vectors^T
				
	 for (uint i=0;i<d;i++)
		 for (uint j=0;j<d;j++) {
			 data[i*(d)+j]=(1.f/eva[i])*eve[j*(d)+i];
		 }
	//r=multiply_Rmn(vectors,d);
	multiply(eve,d,d,data,d,d,return_data);
	//return r;
}

float mvn_pdf(float  *mu, float *eva, float *eve,float *true_mu, uint d);
float mvn_pdf(float  *mu, float *eva, float *eve,float *true_mu, uint d) {


	//struct Rmn *inverse_covariance=theorize_inverse_covariance_Rmn(ed);

	float inverse_covariance[d*d];
	theorize_inverse_covariance(eva,eve,inverse_covariance,d);

	float sum=0,f=powf(1/(2*PI),d*0.5f)*powf(1/theorize_determinant(eva,d),0.5f); //this is working properly..
	

	float first_term[MAX_DIM];
	for (uint i=0;i<d;i++)
		for (uint j=0;j<d;j++)
			first_term[i]+=(mu[j]-true_mu[j])*inverse_covariance[j*d+i];
	for (uint i=0;i<d;i++)
		sum+=first_term[i]*(mu[i]-true_mu[i]);

	return f*expf(-0.5f*sum);
}



__kernel void generate(__constant float *data, __constant uint *n, __constant uint *d, __global float* return_data, __global tinymt32wp_t *d_status)  {

uint id=get_global_id(0);

tinymt32wp_t tiny=init_u01(d_status);

float s2[MAX_DIM*MAX_DIM], eva[MAX_DIM], eve[MAX_DIM*MAX_DIM], mu[MAX_DIM], param[TRIALS][MAX_DIM],last_param[ACCEPTANCES][MAX_DIM], distances[TRIALS], tmp_distances[TRIALS], last_weights[ACCEPTANCES],current_weights[ACCEPTANCES], theorized_data[MAX_DIM*MAX_SAMPLES], d_threshold=0, max_weight=0,sum_weight=0, distance_mean[MAX_DIM];;

//__local float accepted_params[ACCEPTANCES*MAX_DIM];

covariance((float *)data,*n,*d,s2);

svd(s2, *d, eva, eve);

mean((float *)data,*n,*d,mu);

theorize(tiny,d_status,mu, eva, eve, *n,*d, theorized_data);

for (uint g=0;g<GENERATIONS;g++) {
	//##if g==0, we will need to have TRIALS datapioints to use 
	if (g==0) {
		for (uint i=0;i<ACCEPTANCES;i++)
			last_weights[i]=1.f/ACCEPTANCES;
		max_weight=1.f/ACCEPTANCES;

		for (uint i=0, sample=0;i<ACCEPTANCES;i++) {
			sample=u01()*(*n);
			for (uint j=0;j<*d;j++)
				//accepted_params[i*(*d)+j]=data[sample*(*d)+j];
				last_param[i][j]=data[sample*(*d)+j];
		}
	}

	//#double the variance of the last generation & use that as our perturbing variance... BUT I DON'T THINK WE WANT TO CHANGE THE theorize() variance. 
	max_weight=0;
	for (uint i=0;i<ACCEPTANCES;i++) 
		max_weight=MAX(max_weight,last_weights[i]);

	for (uint t=0;t<TRIALS;t++) {
			//#1: take parameter from last acceptances, paying attention to weight
			for (uint i=0;i<*d;i++)
				param[t][i]=mu[i];//SOMETHING;	

			//#2: perturb our parameters
			perturb_mu(tiny, d_status, param[t], eva, eve, 1, *d);
			//#3: theorize a dataset based on perturbed sample
			theorize(tiny,d_status,param[t], eva, eve, *n,*d, theorized_data);
			//#4: record the distance
			distances[t]=distance_f((float*)data,theorized_data,*n,*d);
			tmp_distances[t]=distances[t];;
	}
	//#5: sort the distances and mark the top 10% as accepted
	mergeSort(distances);
	d_threshold=distances[ACCEPTANCES-1];	

	mean(distances,ACCEPTANCES,*d,distance_mean);
	//printf("accepted distance mean is: %f\n",distance_mean[0]);

	//#6: assign accepted data to "accepted_params" matrix
	sum_weight=0,max_weight=0;

	for (uint t=0, acceptances=0;t<TRIALS;t++) {
		if (tmp_distances[t]<d_threshold && acceptances<ACCEPTANCES) { //in case there is a tie on d_threshold, limit to number of acceptances


			for (uint i=0;i<ACCEPTANCES;i++)
				current_weights[acceptances]+=last_weights[i];//*mvn_pdf(param[t],eva,eve,last_param[i], *d);
			//current_weights[acceptances]=mvn_pdf(param[t],eva,eve,mu,*d)/current_weights[acceptances];
			max_weight=MAX(max_weight,current_weights[acceptances]);
			sum_weight+=current_weights[acceptances];
			for (uint i=0;i<*d;i++) 
				last_param[acceptances][i]=param[t][i];
				//accepted_params[acceptances*(*d)+i]=param[t][i];
			
			acceptances++;
		}
	}

	for (uint t=0;t<ACCEPTANCES;t++) //FIXME .. put the scale back in
		last_weights[t]=current_weights[t];///sum_weight/max_weight; //scale down to 1.

	//#7: calculate weights:
}
/*
float A[4*6], B[6*4], C[4*4];

printf("here is the matrix A\n");
for (uint i=0;i<4;i++) {
	for (uint j=0;j<6;j++) {
		if (i==j)
			A[i*6+j]=1;
		printf(" %f",A[i*6+j]);
	
	}
	printf("\n");
}
printf("here is the matrix B\n");
for (uint i=0;i<6;i++) {
	for (uint j=0;j<4;j++) {
		if (i==j)
		B[i*4+j]=1;
		printf(" %f",B[i*4+j]);	
	}
	printf("\n");
}
printf("here is the matrix C\n");
multiply(A,4,6,B,6,4,C);
for (uint i=0;i<4;i++) {
	for (uint j=0;j<4;j++) {
		printf(" %f",C[i*4+j]);
	}
	printf("\n");
}	

float true_mu[1];
true_mu[0]=6.7;
printf("mu=%f,eva=%f,eve=%f,true_mu=%f,MVN=%f\n",mu[0],eva[0],eve[0],true_mu[0],mvn_pdf(mu, eva, eve,true_mu,*d));
*/

for (uint i=0;i<ACCEPTANCES*MAX_DIM;i++)
	return_data[i]=last_weights[i];	

}
